# transform.py

r'''3D coordinate transformations.

This uses a right-handed coordinate system.

This is based on matrix multiplication, which, for each position in the
result, multiplies each element of each row in the first matrix by the
corresponding element from the column in the second matrix and summing the
results:

        +-     -+           +-     -+            +-                        -+
        | a1 a2 |           | c1 c2 |            | a1c1 + a2d1  a1c2 + a2d2 |
    A = |       |  and  B = |       |, then AB = |                          |
        | b1 b2 |           | d1 d2 |            | b1c1 + b2d1  b1c2 + b2d2 |
        +-     -+           +-     -+            +-                        -+

Some examples:

To convert X,Y,Z from global to local coordinates:

    +-           -+  +- -+     +-                    -+
    | a1 a2 a3 a4 |  | X |     | a1X + a2Y + a3Z + a4 |
    | b1 b2 b3 b4 |  | Y |  =  | b1X + b2Y + b3Z + b4 |
    | c1 c2 c3 c4 |  | Z |     | c1X + c2Y + c3Z + c4 |
    |  0  0  0  1 |  | 1 |     |           1          |
    +-           -+  +- -+     +-                    -+

Translation matrix:

    +-       -+  +- -+     +-     -+
    | 1 0 0 A |  | X |     | X + A |
    | 0 1 0 B |  | Y |  =  | Y + B |
    | 0 0 1 C |  | Z |     | Z + C |
    | 0 0 0 1 |  | 1 |     |   1   |
    +-       -+  +- -+     +-     -+

Rotate on X matrix:

    +-        -+  +- -+     +-       -+
    | 1 0  0 0 |  | X |     |    X    |
    | 0 c -s 0 |  | Y |  =  | cY - sZ |
    | 0 s  c 0 |  | Z |     | sY + cZ |
    | 0 0  0 1 |  | 1 |     |    1    |
    +-        -+  +- -+     +-       -+

    +-        -+  +-       -+     +-                -+
    | 1 0  0 0 |  | 1 0 0 A |     | 1  0  0     A    |
    | 0 c -s 0 |  | 0 1 0 B |  =  | 0  c -s  cB - sC |
    | 0 s  c 0 |  | 0 0 1 C |     | 0  s  c  sB + cC |
    | 0 0  0 1 |  | 0 0 0 1 |     | 0  0  0     1    |
    +-        -+  +-       -+     +-                -+

    +-       -+  +-        -+     +-        -+
    | 1 0 0 A |  | 1 0  0 0 |     | 1 0  0 A |
    | 0 1 0 B |  | 0 c -s 0 |  =  | 0 c -s B |
    | 0 0 1 C |  | 0 s  c 0 |     | 0 s  c C |
    | 0 0 0 1 |  | 0 0  0 1 |     | 0 0  0 1 |
    +-       -+  +-        -+     +-        -+


'''

from __future__ import division

import sys
import math
import operator
import numpy
#from Scientific.Geometry.Transformation import Translation, Rotation
#import Scientific.Geometry

from ot_simulator import euler_angles

debug = True

def sin(deg):
    return math.sin(math.radians(deg))

def cos(deg):
    return math.cos(math.radians(deg))

def sin_cos(deg):
    rad = math.radians(deg)
    return math.sin(rad), math.cos(rad)

def column_vector(x=0.0, y=0.0, z=0.0):
    return numpy.matrix(((float(x),),
                         (float(y),),
                         (float(z),),
                         (1.0,)))

def to_point(column_vector):
    r'''Converts a column_vector (4x1 matrix) into an X, Y, Z point.

        >>> print_array(to_point(column_vector(1,2,3)))
        (1.0, 2.0, 3.0)
    '''
    return column_vector.A1[:3]

#X_axis = Vector(1, 0, 0)
#Y_axis = Vector(0, 1, 0)
#Z_axis = Vector(0, 0, 1)
#Axes=(X_axis, Y_axis, Z_axis)
#NullVector = Vector(0, 0, 0)

X = 0
Y = 1
Z = 2
Default_rotation_order = 'YXZ'

class transform(object):
    r'''
        A transform is an immutable object representing a coordinate system
        transformation from a "global" coordinate system to a "local"
        coordinate system.

        The inverse transforms from the "local" coordinate system to the
        "global" coordinate system.

        All moves and rotates on transforms are relative to the "local"
        coordinate system (and return new transforms).

        Given two transforms:
            T1 from G1 -> L1
            T2 from G2 -> L2

        T1.add(T2) results in the transform from G1 -> L2 with L1 == G2.
        Think of it like G1 -> (L1 == G2) -> L2.

        Note that T2.add(T1) is a very different transform!  It is G2 -> L1.

        And T1.subtract(T2) results in the transform from L2 -> L1 with G1 ==
        G2.  Calling this result transform R, we have T2.add(R) == T1 with G1
        == G2.  Think of this addition as G2 -> L2 -> L1, which with G1 == G2
        is just G1 -> L1 or T1.

        Again, T2.subtract(T1) is different than T1.subtract(T2) resulting in
        L1 -> L2!

        So there are four ways to combine the two transforms, T1 and T2:

            T1                  G1 -> L1
            T2                  G2 -> L2
            T1.add(T2)          G1 -> L2 (L1 == G2)
            T2.add(T1)          G2 -> L1 (L2 == G1)
            T1.subtract(T2)     L2 -> L1 (G1 == G2)
            T2.subtract(T1)     L1 -> L2 (G1 == G2)

        >>> t1 = Identity_transform.move(1,2,3)
        >>> print_array(t1.global_to_local((1,2,3)))
        (0.0, 0.0, 0.0)
        >>> print_array(t1.global_to_local((1,1,1)))
        (0.0, -1.0, -2.0)
        >>> print_array(t1.local_to_global())
        (1.0, 2.0, 3.0)
        >>> t1
        <transform (1.0, 2.0, 3.0)>

        >>> t1.inverse()
        <transform (-1.0, -2.0, -3.0)>

        >>> t2 = Identity_transform.rotate_x(90.0)
        >>> print_array(t2.global_to_local((1,2,3)))
        (1.0, 3.0, -2.0)

        >>> t2 = t1.rotate_x(90.0)
        >>> print_array(t2.global_to_local((1,2,3)))
        (0.0, 0.0, 0.0)
        >>> print_array(t2.local_to_global())
        (1.0, 2.0, 3.0)
        >>> print_array(t2.global_to_local((1,1,1)))
        (0.0, -2.0, 1.0)
        >>> t2
        <transform (1.0, 2.0, 3.0)@(90.0, 0.0, 0.0)>

        >>> t2.inverse()
        <transform (-1.0, -3.0, 2.0)@(-90.0, 0.0, 0.0)>

        >>> t1 = t1.move(1,2,3)
        >>> print_array(t1.global_to_local((2,4,6)))
        (0.0, 0.0, 0.0)
        >>> print_array(t1.global_to_local((1,1,1)))
        (-1.0, -3.0, -5.0)
        >>> print_array(t1.local_to_global())
        (2.0, 4.0, 6.0)
        >>> t1
        <transform (2.0, 4.0, 6.0)>

        >>> t3 = t1.add(t2)
        >>> print_array(t3.global_to_local((3,6,9)))
        (0.0, 0.0, 0.0)
        >>> print_array(t3.global_to_local((1,1,1)))
        (-2.0, -8.0, 5.0)
        >>> print_array(t3.local_to_global())
        (3.0, 6.0, 9.0)
        >>> t3
        <transform (3.0, 6.0, 9.0)@(90.0, 0.0, 0.0)>

        >>> t3 = t2.add(t1)
        >>> print_array(t3.global_to_local((3,-4,7)))
        (0.0, 0.0, 0.0)
        >>> print_array(t3.global_to_local((1,1,1)))
        (-2.0, -6.0, -5.0)
        >>> print_array(t3.local_to_global())
        (3.0, -4.0, 7.0)
        >>> t3
        <transform (3.0, -4.0, 7.0)@(90.0, 0.0, 0.0)>

        >>> t3 = t1.subtract(t2)
        >>> print_array(t3.global_to_local((1,3,-2)))
        (0.0, 0.0, 0.0)
        >>> print_array(t3.global_to_local((1,1,1)))
        (0.0, -3.0, -2.0)
        >>> print_array(t3.local_to_global())
        (1.0, 3.0, -2.0)
        >>> t3
        <transform (1.0, 3.0, -2.0)@(-90.0, 0.0, 0.0)>

        >>> t2.subtract(t1)
        <transform (-1.0, -2.0, -3.0)@(90.0, 0.0, 0.0)>

        >>> t3 = t2.move(1,2,3)
        >>> t3
        <transform (2.0, -1.0, 5.0)@(90.0, 0.0, 0.0)>

        >>> t2.rotate_y(90.0)
        <transform (1.0, 2.0, 3.0)@(0.0, 90.0, 90.0)>
    '''

    def __init__(self, matrix):
        self.matrix = matrix

    @classmethod
    def translation(cls, x=0.0, y=0.0, z=0.0):
        return cls(numpy.matrix(((1.0, 0.0, 0.0, x),
                                 (0.0, 1.0, 0.0, y),
                                 (0.0, 0.0, 1.0, z),
                                 (0.0, 0.0, 0.0, 1.0))))

    @classmethod
    def rotation_on_x(cls, deg):
        s, c = sin_cos(deg)
        return cls(numpy.matrix(((1.0, 0.0, 0.0, 0.0),
                                 (0.0,   c,  -s, 0.0),
                                 (0.0,   s,   c, 0.0),
                                 (0.0, 0.0, 0.0, 1.0))))

    @classmethod
    def rotation_on_y(cls, deg):
        s, c = sin_cos(deg)
        return cls(numpy.matrix(((  c, 0.0,   s, 0.0),
                                 (0.0, 1.0, 0.0, 0.0),
                                 ( -s, 0.0,   c, 0.0),
                                 (0.0, 0.0, 0.0, 1.0))))

    @classmethod
    def rotation_on_z(cls, deg):
        s, c = sin_cos(deg)
        return cls(numpy.matrix(((  c,  -s, 0.0, 0.0),
                                 (  s,   c, 0.0, 0.0),
                                 (0.0, 0.0, 1.0, 0.0),
                                 (0.0, 0.0, 0.0, 1.0))))

    def __repr__(self):
        t = self.local_to_global()
        r = self.euler_angles()
        if abs(r[0]) < 1e-10 and abs(r[1]) < 1e-10 and abs(r[2]) < 1e-10:
            return "<transform %s>" % (print_array(t, ''),)
        if abs(t[0]) < 1e-10 and abs(t[1]) < 1e-10 and abs(t[2]) < 1e-10:
            return "<transform @%s>" % (print_array(r, ''),)
        return "<transform %s@%s>" % (print_array(t, ''), print_array(r, ''))

    def euler_angles(self, order = Default_rotation_order):
        r'''Returns Euler angles for rotation in specified axis ``order``.

        ``order`` is one of six 3-char permutations of the letters:
        'X', 'Y', and 'Z'.  Repeated axes are not supported.

        Angles are returned in X, Y, Z order regardless of ``order`` parameter.
        Angles are in degrees.

            >>> r1 = Identity_transform.rotate_y(10.0)
            >>> r2 = r1.rotate_x(20.0)
            >>> r3 = r2.rotate_z(30.0)
            >>> print_array(r3.euler_angles('YXZ'))
            (20.0, 10.0, 30.0)
        '''
        r = euler_angles.Eul_FromHMatrix(self.matrix, order)
        return tuple(r[i]
                     for i, o in sorted(enumerate(order),
                                        key=operator.itemgetter(1)))

    def global_to_local(self, global_point = (0.0, 0.0, 0.0)):
        r'''Converts a ``global_point`` to a local_point.
        '''
        return to_point(self.matrix.I * column_vector(*global_point))

    def local_to_global(self, local_point = (0.0, 0.0, 0.0)):
        r'''Converts a ``local_point`` to a global_point.
        '''
        return to_point(self.matrix * column_vector(*local_point))

    def add(self, transformation):
        r'''Adds ``transformation`` to self.

        If self is G -> L1 and ``transformation`` is L1 -> L2, then
        self.add(transformation) is G -> L2.

        This is `not` commutative, but it `is` associative.
        '''
        return transform(self.matrix * transformation.matrix)

    def inverse(self):
        r'''Reverses the mapping.
 
        Converts G -> L into L -> G.
        '''
        return transform(self.matrix.I)

    def subtract(self, transformation):
        r'''Subtracts ``transformation`` from self.

        If self is G -> L1 and ``transformation`` is G -> L2, then
        self.subtract(transformation) is L2 -> L1.
        '''
        return transformation.inverse().add(self)

    def move(self, x=0.0, y=0.0, z=0.0):
        '''
            Translates (moves) the coordinate system by a delta vector amount.
        '''
        return self.add(transform.translation(x, y, z))

    def rotate_x(self, degrees):
        ''' Rotates the coordinate system on its X axis.  Positive rotation is
            counter-clockwise looking from positive X axis back to Y, Z plane.
        '''
        return self.add(transform.rotation_on_x(degrees))

    def rotate_y(self, degrees):
        ''' Rotates on Y axis.  Positive rotation is counter-clockwise looking
            from positive Y axis back to Z, X plane.
        '''
        return self.add(transform.rotation_on_y(degrees))

    def rotate_z(self, degrees):
        ''' Rotates on Z axis.  Positive rotation is counter-clockwise looking
            from positive Z axis back to X, Y plane.
        '''
        return self.add(transform.rotation_on_z(degrees))

#: This is the starting transform.  It could be thought of as the origin.
Identity_transform = transform.translation()

def to_str(n):
    if abs(n) > 1e-10: return str(n)
    return '0.0'

def print_array(v, f=None):
    '''
        >>> print_array(0.2)
        0.2
        >>> print_array((1,))
        (1)
        >>> print_array((1,2,3))
        (1, 2, 3)
        >>> print_array(((1,2,3),(4,5,6)))
        ((1, 2, 3), (4, 5, 6))
        >>> print_array(numpy.array((1.0,2.0,3.0)))
        (1.0, 2.0, 3.0)
    '''
    if f is None: f = sys.stdout
    elif f == '':
        import StringIO
        c = StringIO.StringIO()
        print_array(v, c)
        ans = c.getvalue()
        c.close()
        return ans
    if isinstance(v, (tuple, numpy.ndarray)):
        f.write('(')
        first = True
        for x in v:
            if first: first = False
            else: f.write(', ')
            print_array(x, f)
        f.write(')')
    else:
        f.write(to_str(v))


def test():
    import doctest
    import sys
    sys.exit(doctest.testmod()[0])

if __name__ == "__main__":
    test()
